#include <buola/buola.h>
#include <buola/cv/cdisptracker.h>
#include <buola/image/algorithm/edges.h>

#pragma GCC diagnostic ignored "-Wdouble-promotion"
///\todo fix once we have CRect_f and so on

namespace buola { namespace cvi {

/*
static const float sInitScale[2]={1.0f,0.0f};
static const float sInitZRot[2]={0.0f,0.0f};
static const float sInitXDif[2]={0.0f,0.0f};
static const float sInitYDif[2]={0.0f,0.0f};
static const float sInitXPos[2]={160.0f,0.0f};
static const float sInitYPos[2]={120.0f,0.0f};
*/

CDispTracker::CDispTracker() 
//    :	mFocal(2200.0) //foveal focal distance
    :	mLeftThresh((mMinThresh+mMaxThresh)/2.0f)
    ,	mRightThresh((mMinThresh+mMaxThresh)/2.0f)
/*
    ,  	mCount(0)
    ,	mSubSample(1)
    ,	mWithDisp(false)
    ,	mTracking(false)
    ,	mNumValid(0)
    ,	mExpRange(40.0)
    ,	mExpSize(300.0,300.0)
    ,	mKalScale(sInitScale,0.0001,0.01)
    ,	mKalZRot(sInitZRot,0.0001,0.1) //should change it because it's now radians
    ,	mKalXDif(sInitXDif,4.0,4.0)
    ,	mKalYDif(sInitYDif,4.0,4.0)
    ,	mKalXPos(sInitXPos,100.0,16.0)
    ,	mKalYPos(sInitYPos,100.0,16.0)
    ,	mKalXDifCenter(sInitXDif,4.0,4.0)
    ,	mKalXPosCenter(sInitXPos,100.0,16.0)
    ,	mKalYPosCenter(sInitYPos,100.0,16.0)
*/
{  
} 
  
void CDispTracker::GetParameters(float &pXDif, float &pYDif, float &pZRot, float &pScale)
{
    pXDif=mXDif;
    pYDif=mYDif;
    pZRot=mZRot;
    pScale=mScale;
/*
    if(mKalXPos.GetVariance()<mKalXPosCenter.GetVariance()) 
        pXDif = mKalXDif[0] * mResolution;
    else 
        pXDif = mKalXDifCenter[0] * mResolution;
    pYDif = mKalYDif[0] * mResolution;
    pZRot = mKalZRot[0];
    pScale = mKalScale[0];
*/
}  
  
CPoint_d CDispTracker::GetCenter()
{
/*
    if(mKalXPos.GetVariance()<mKalXPosCenter.GetVariance()) 
        pCenter.Set(mKalXPos[0]*mResolution,mKalYPos[0]*mResolution);
    else
        pCenter.Set(mKalXPosCenter[0]*mResolution,mKalYPosCenter[0]*mResolution);
*/
    ///\todo
    return CPoint_d(0,0);
}   

void CDispTracker::Update(const img::CView_gray8 &pLeft,const img::CView_gray8 &pRight,
                          float /*pPanPred*/, float /*pTiltPred*/, float /*pVergPred*/)
{
    ///\todo use or remove parameters
    int lW=pLeft.Width();
    int lH=pLeft.Height();

    std::vector<std::pair<CPoint_d,float>> lLPeaks,lRPeaks;
//    mPrevCornsL.Move(mCurrCornsL);
//    mPrevCornsR.Move(mCurrCornsR);
    harris_corners(pLeft,lLPeaks,true,mLeftThresh);
    harris_corners(pRight,lRPeaks,true,mRightThresh);
    mLCorners.Init(lLPeaks);
    mRCorners.Init(lRPeaks);
    mLCorners.CreatePatches(pLeft);
    mRCorners.CreatePatches(pRight);
    match_corners(mLCorners,mRCorners,mStereoMatch,CStereoCornerMatcher());

//    img::CImage_gray8 lSImage(w,h);
//    img::CImage_gray32f lFImage(w,h);
/*
    MotionMatcher lMMatchL(mCurrCornsL, mPrevCornsL);
    lMMatchL.MatchCorners(lLImg, lLImg, mFocal/mResolution, 0.02);
#if !DISPTRACK_LEFTONLY
    MotionMatcher mmatchr(mCurrCornsR, mPrevCornsR);
    mmatchr.MatchCorners(lRImg, lRImg, mFocal/mResolution, 0.02);
#endif
*/

    mLeftThresh*=(0.8f + 0.2f*mLCorners.size()/mExpNumPts);
    mRightThresh*=(0.8f + 0.2f*mLCorners.size()/mExpNumPts);

    FindRelativePose(lW,lH,mXDif,mYDif,mZRot,mScale);

#if 0
    float lXPos, lYPos, lXPosCenter, lYPosCenter, lXDifCenter;
    if (/*!tracking &&*/ FindCenter(mCurrCornsL, w/mResolution, h/mResolution, lYDif, lXPosCenter, lYPosCenter, lXDifCenter, 0.1))
    {
        lXDif = lXDifCenter;
        lXPos = lXPosCenter;
        lYPos = lYPosCenter;
    }

    bool lTransfer = false;

    if (mTracking)
    {
        if (AffineTransfer(mPrevCornsL, mPrevCornsR, w/mResolution, h/mResolution))
        {
            if (fabs(lXPos-mLeftPos.x)>w/8 || fabs(lXPos-mLeftPos.x)>h/8)
            {
                mLeftPos.x = (5.0*mLeftPos.x + 1.0*lXPos) / 6.0;
                mLeftPos.y = (5.0*mLeftPos.y + 1.0*lYPos) / 6.0;
            }
            lXPosCenter = mLeftPos.x;
            lYPosCenter = mLeftPos.y;
            lTransfer = true;
        }
        else
        {
            mLeftPos.x = lXPosCenter;
            mLeftPos.y = lYPosCenter;
            mRightPos.x = lXPosCenter - lXDif;
            mRightPos.y = lYPosCenter - lYDif;
        }
    }

    bool lTransform = mWithDisp;

    if (lTransform)
    {
        int er = (int)(mExpRange / mSubSample);
        int s = mResolution;
        Image<unsigned char> lTImg(w/s, h/s);
        while (s<mSubSample) {
            s *= 2;
            lTImg.SetSize(w/s, h/s);
            ScaleDown(lLImg, lTImg);
            lLImg.SetSize(w/s, h/s);
            Copy(lTImg, lLImg);
            ScaleDown(lRImg, lTImg);
            lRImg.SetSize(w/s, h/s);
            Copy(lTImg, lRImg);
        }
        Normalize(lRImg, lLImg);
        TransformImage(lRImg, lTImg, mKalYDif[0]*mResolution/s, mKalZRot[0], mKalScale[0]);
        Image<float> lDImg(w/s, h/s);
        if (mWithDisp)
        {
            Clear(lDImg);
            CalcDisparity(lLImg, lTImg, lDImg, -2*er, 2*er, 17, 10.0, true, false);
            DispSegment(lDImg, lTImg, 4*er, er, lXPos, lYPos);
        }
    }

    float lDXPos = -pPanPred*mFocal/2.0;
    float lDYPos = -pTiltPred*mFocal/2.0;
    float lDXDif = -pVergPred*mFocal/2.0;
    //msg_info() << mKalYDif[0] << " " << mKalZRot[0] << " " << mKalScale[0] << "\n";
    Info() << "num valid=" << mNumValid << "\n";
    if (mNumValid>7)
        KalmanUpdate(lXDif, lYDif, lZRot, lScale, lXPos, lYPos, lXPosCenter, lYPosCenter, lXDifCenter, lDXPos, lDYPos, lDXDif);

    msg_info() << std::setprecision(3) << mNumValid << "  " <<
        "lXPos: " << (int)lXPos << " " << (int)mKalXPos[0] <<" "<< sqrt(mKalXPos.GetVariance()) <<
        "  lYPos: " << (int)lYPos << " " << (int)mKalYPos[0] <<" "<< sqrt(mKalYPos.GetVariance()) <<
        "  lXDif: " << lXDif << " " << mKalXDif[0]<<" "<< sqrt(mKalXDif.GetVariance()) <<
        "  lYDif: " << lYDif << " " << mKalYDif[0]<<" "<< sqrt(mKalYDif.GetVariance()) <<
        "  lZRot: " << lZRot << " " << mKalZRot[0]<<" "<< sqrt(mKalZRot.GetVariance()) <<
        "  lScale: " << lScale << " " << mKalScale[0]<<" "<< sqrt(mKalScale.GetVariance()) << "  " << std::setprecision(6);
    if (lTransfer)
        msg_info() << "transfer\n";
    else
        if (mTracking)
            msg_info() << "bootstrap\n";
        else
            msg_info() << "update\n";
    mCount++;
#endif
}

int CDispTracker::FindSimilarityMatrix()
{
    const float lLimit = 9.0;
    const int lMinNum = 6;

    if(mStereoMatch.size()<lMinNum)
    {
        msg_warn() << "FindSimilarityMatrix(): Too few successful matches\n";
        mParams[0] = mParams[2] = mParams[4] = 0.0;
        mParams[1] = 1.0;
        mParams[2] = -1.0;
        return mStereoMatch.size();
    }

    int lMaxFit = 0;
    float a[5], ba[5];

    for (int lLoop=0;lLoop<200;lLoop++)
    {  // Use RANSAC to find initial estimate
        int lIdx[lMinNum];
        int n = 0;

        while (n<lMinNum)
        {
            lIdx[n] = rand()%mStereoMatch.size();
            bool lExist = false;
            for (int i=0;i<n;i++)
                if (lIdx[n]==lIdx[i])
                    lExist = true;
            if (!lExist)
                n++;
        }

        //  y2 + a[0]*x1 + a[1]*y1 + a[4] = 0

        // 0.0  0.0   0
        // 0.0  0.0   1
        // a[0] a[1] a[4]

        float lMed[3];
        for (int i=0;i<3;i++)
            lMed[i] = 0.0;
        for (int i=0;i<lMinNum;i++)
        {
            CCorner &lL = mLCorners[mStereoMatch[lIdx[i]].mL];
            CCorner &lR = mRCorners[mStereoMatch[lIdx[i]].mR];
            lMed[0] += lL.mPixPos.x;
            lMed[1] += lL.mPixPos.y;
            lMed[2] += lR.mPixPos.y;
        }
        for (int i=0;i<3;i++)
            lMed[i] /= lMinNum;

        float lCXX = 0.0f, lCXY = 0.0f, lCYY = 0.0f;
        float lDX = 0.0f, lDY = 0.0f;
        for (int i=0;i<lMinNum;i++)
        {
            CCorner &lL = mLCorners[mStereoMatch[lIdx[i]].mL];
            CCorner &lR = mRCorners[mStereoMatch[lIdx[i]].mR];
            float lX1 = lL.mPixPos.x - lMed[0];
            float lY1 = lL.mPixPos.y - lMed[1];
            float lY2 = lR.mPixPos.y - lMed[2];
            lCXX += lX1*lX1;
            lCXY += lX1*lY1;
            lCYY += lY1*lY1;
            lDX += lX1*lY2;
            lDY += lY1*lY2;
        }

        float lDet = lCXX*lCYY - lCXY*lCXY;
        a[0] = lCYY*lDX - lCXY*lDY;
        a[1] = lCXX*lDY - lCXY*lDX;
        if (lDet!=0.0f)
        {
            a[0] /= lDet;
            a[1] /= lDet;
        }
        a[4] = lMed[2] - a[0]*lMed[0] - a[1]*lMed[1];

        int lNumFit = 0;
        for (int i=0;i<mStereoMatch.size();i++)
        {
            CCorner &lL = mLCorners[mStereoMatch[i].mL];
            CCorner &lR = mRCorners[mStereoMatch[i].mR];
            float lErr = lL.mPixPos.x*a[0] + lL.mPixPos.y*a[1] - lR.mPixPos.y + a[4];
            if (lErr*lErr<lLimit)
                lNumFit++;
        }
        a[2] = 0.0f;
        a[3] = 1.0f;

        //choose the one that fits more... another option is to choose the one with the minimum error, of the ones that fit many
        if (lNumFit>lMaxFit)
        {
            for (int i=0;i<5;i++)
                ba[i] = a[i];
            lMaxFit = lNumFit;
        }
    }

    int lNumFit = 0;       // Improve similarity F-matrix using M-estimators
    for (int lLoop=0;lLoop<3;lLoop++)
    {
        float lMed[3];
        for (int i=0;i<3;i++)
            lMed[i] = 0.0;

        lNumFit = 0;
        for (int i=0;i<mStereoMatch.size();i++)
        {
            CCorner &lL = mLCorners[mStereoMatch[i].mL];
            CCorner &lR = mRCorners[mStereoMatch[i].mR];
            float lErr = lL.mPixPos.x*ba[0] + lL.mPixPos.y*ba[1] - lR.mPixPos.y + ba[4];
            if (lErr*lErr<lLimit)
            {
                lMed[0] += lL.mPixPos.x;
                lMed[1] += lL.mPixPos.y;
                lMed[2] += lR.mPixPos.y;
                lNumFit++;
            }
        }
        for (int i=0;i<3;i++)
            lMed[i] /= lNumFit;

        float lCXX = 0.0f, lCXY = 0.0f, lCYY = 0.0f;
        float lDX = 0.0f, lDY = 0.0f;
        for (int i=0;i<mStereoMatch.size();i++)
        {
            CCorner &lL = mLCorners[mStereoMatch[i].mL];
            CCorner &lR = mRCorners[mStereoMatch[i].mR];
            float lErr = lL.mPixPos.x*ba[0] + lL.mPixPos.y*ba[1] - lR.mPixPos.y + ba[4];
            if (lErr*lErr<lLimit)
            {
                float lX1 = lL.mPixPos.x - lMed[0];
                float lY1 = lL.mPixPos.y - lMed[1];
                float lY2 = lR.mPixPos.y - lMed[2];
                lCXX += lX1*lX1;
                lCXY += lX1*lY1;
                lCYY += lY1*lY1;
                lDX += lX1*lY2;
                lDY += lY1*lY2;
            }
        }

        float lDet = lCXX*lCYY - lCXY*lCXY;
        ba[0] = lCYY*lDX - lCXY*lDY;
        ba[1] = lCXX*lDY - lCXY*lDX;
        if (lDet!=0.0f)
        {
            ba[0] /= lDet;
            ba[1] /= lDet;
        }
        ba[4] = lMed[2] - ba[0]*lMed[0] - ba[1]*lMed[1];
    }

    mParams[0] = ba[0];
    mParams[1] = ba[1];
    mParams[2] = -ba[2];
    mParams[3] = -ba[3];
    mParams[4] = ba[4];
    return lNumFit;
}

int CDispTracker::FindRelativePose(int pW, int pH, float &pXDif, float &pYDif, float &pZRot, float &pScale)
{
    //int lNumFit = FindAffineFMatrix(lLCornList);
    FindSimilarityMatrix();
    //msg_info() << lNumFit << " " << mParams[0] << " " << mParams[1] << " " << mParams[2] << " " << mParams[3] << " " << mParams[4] << "\n";
    float lALen = mParams[0]*mParams[0] + mParams[1]*mParams[1];
    float lBLen = mParams[2]*mParams[2] + mParams[3]*mParams[3];
    pScale = sqrt(lBLen/lALen);

    mTrans[0] = -(mParams[0]*mParams[2] + mParams[1]*mParams[3]) / lALen;
    mTrans[1] = -(mParams[0]*mParams[3] - mParams[1]*mParams[2]) / lALen;
    mTrans[2] = mParams[1] / sqrt(lALen);
    mTrans[3] = -mParams[0]*mParams[4] / lALen;
    mTrans[4] = -mTrans[1];    // Find affine projection
    mTrans[5] = mTrans[0];
    mTrans[6] = -mParams[0] / sqrt(lALen);
    mTrans[7] = -mParams[1]*mParams[4] / lALen;
    pZRot = atan2(mTrans[1], mTrans[0]);

    float *dxs = new float[mStereoMatch.size()];
    float *dys = new float[mStereoMatch.size()];

    int lNum = 0;             //   Residual after similarity

    for (int i=0;i<mStereoMatch.size();i++)
    {
        CCorner &lL = mLCorners[mStereoMatch[i].mL];
        CCorner &lR = mRCorners[mStereoMatch[i].mR];

        float x = lR.mPixPos.x - pW/2;
        float y = lR.mPixPos.y - pH/2;
        float rx = x*mTrans[0] + y*mTrans[1] + pW/2;
        float ry = x*mTrans[4] + y*mTrans[5] + pH/2;
        dxs[lNum] = lL.mPixPos.x - rx;
        dys[lNum] = lL.mPixPos.y - ry;
        lNum++;
    }

    pXDif = 0.0;              // Find pXDif and pYDif using medians
    pYDif = 0.0;
    float lDelta = 100.0;
    for (int i=0;i<12;i++)
    {
        int lNumXDif = 0;
        int lNumYDif = 0;
        for (int j=0;j<mStereoMatch.size();j++)
        {
            if (dxs[j]>pXDif)
                lNumXDif++;
            if (dys[j]>pYDif)
                lNumYDif++;
        }

        if (lNumXDif>mStereoMatch.size()/2)
            pXDif += lDelta;
        else
            pXDif -= lDelta;

        if (lNumYDif>mStereoMatch.size()/2)
            pYDif += lDelta;
        else
            pYDif -= lDelta;
        lDelta /= 2.0;
    }

    mTrans[3] = pXDif;
    mTrans[7] = pYDif;

    //for (int i=0;i<lNum;i++)
    //  msg_info() << i << " " << std::setprecision(3) << dxs[i]-pXDif << " " << dys[i]-pYDif << std::setprecision(6) << "\n";

    for (int k=0;k<3;k++)
    {  // Improve translation and epipolar line
        float lSumX = 0.0, lSumY = 0.0, lWeightSum = 0.0;
        float lSumXX = 0.0, lSumXY = 0.0, lSumYY = 0.0;

        for (int i=0;i<lNum;i++)
        {
            float lErr = (dxs[i]-mTrans[3])*mTrans[6] - (dys[i]-mTrans[7])*mTrans[2];
            float lWei = 4.0 / (4.0 + lErr*lErr); // Weird error!!

            //float lWei = expyerr*expyerr / (expyerr*expyerr + er*er);
            lSumX += lWei * dxs[i];
            lSumY += lWei * dys[i];
            lSumXX += lWei * dxs[i] * dxs[i];
            lSumXY += lWei * dxs[i] * dys[i];
            lSumYY += lWei * dys[i] * dys[i];
            lWeightSum += lWei;
        }

        lSumX /= lWeightSum;
        lSumY /= lWeightSum;
        lSumXX /= lWeightSum;
        lSumXY /= lWeightSum;
        lSumYY /= lWeightSum;
        lSumXX -= lSumX * lSumX;
        lSumXY -= lSumX * lSumY;
        lSumYY -= lSumY * lSumY;
        float lEig = (lSumXX + lSumYY) / 2;
        lEig += sqrt(lEig*lEig - (lSumXX*lSumYY - lSumXY*lSumXY));
        mTrans[2] = lSumXY;
        mTrans[6] = lEig - lSumXX;
        float lMagn = sqrt(mTrans[2]*mTrans[2] + mTrans[6]*mTrans[6]);
        mTrans[2] /= lMagn;
        mTrans[6] /= lMagn;
        mTrans[3] = lSumX;
        mTrans[7] = lSumY;
    }
    pXDif = mTrans[3];
    pYDif = mTrans[7];

    lNum = 0;
    int lShouldRemove=0;
    for (int i=0;i<mStereoMatch.size();i++)
    {
        CCorner &lL = mLCorners[mStereoMatch[i].mL];
        CCorner &lR = mRCorners[mStereoMatch[i].mR];

        float x = lL.mPixPos.x - pW/2;
        float y = lL.mPixPos.y - pH/2;
        float rx = x*mTrans[0] + y*mTrans[1] + mTrans[3] + pW/2;
        float ry = x*mTrans[4] + y*mTrans[5] + mTrans[7] + pH/2;
        float dx = lL.mPixPos.x - rx;
        float dy = lL.mPixPos.y - ry;
        float er = dx*mTrans[6] - dy*mTrans[2];
        float cz = dx*mTrans[2] + dy*mTrans[6];

        //if (er>expyerr || er<-expyerr) { //Weird error!!
        if (er>3.0 || er<-3.0)
        {
            lShouldRemove++;
        }
        else
        {
            lL.mCZ=cz;
            lR.mCZ=cz;
            lNum++;
            //msg_info() << i << ": " << cz << " " << er << "\n";
        }
    }

    delete [] dxs;
    delete [] dys;
    return lNum;
}

#if 0
void CDispTracker::KalmanUpdate(float pXDif, float pYDif, float pZRot, float pScale, 
  float pXPos, float pYPos, float pXPosCenter, float pYPosCenter, float pXDifCenter, 
  float pPXPos, float pPYPos,float pPXDif)
{
    if (!mCount)
    { 
        mKalYDif.GetState()[0] = pYDif;
        mKalScale.GetState()[0] = pScale;
        mKalZRot.GetState()[0] = pZRot;
        mKalXDif.GetState()[0] = pXDif;
        mKalXPos.GetState()[0] = pXPos;
        mKalYPos.GetState()[0] = pYPos;
        mKalXDifCenter.GetState()[0] = pXDifCenter;
        mKalXPosCenter.GetState()[0] = pXPosCenter; 
        mKalYPosCenter.GetState()[0] = pYPosCenter;
    } 
    else
    { 
        mKalYDif.Update(&pYDif, nullptr, mKalYDif.Accept(&pYDif, 2.0f));
        mKalScale.Update(&pScale, nullptr, mKalScale.Accept(&pScale, 2.0f));
        mKalZRot.Update(&pZRot, nullptr, mKalZRot.Accept(&pZRot, 2.0f));
        mKalXDif.Update(&pXDif, &pPXDif);
        mKalXPos.Update(&pXPos, &pPXPos);
        mKalYPos.Update(&pYPos, &pPYPos);
        mKalXDifCenter.Update(&pXDifCenter, &pPXDif);
        mKalXPosCenter.Update(&pXPosCenter, &pPXPos); 
        mKalYPosCenter.Update(&pYPosCenter, &pPYPos);
  }
}
  
bool CDispTracker::AffineTransfer(Corners &pLCorners, Corners &pRCorners, int pW, int pH)
{
//used only for tracking!
#if 0
    float x = mRightPos[0] - w/2;
    float y = mRightPos[1] - h/2;
    float rx = x*mTrans[0] + y*mTrans[1] + mTrans[3] + w/2;
    float ry = x*mTrans[4] + y*mTrans[5] + mTrans[7] + h/2;
    float cz = 0.0; //(mLeftPos.x - rx)*mTrans[2] + (mLeftPos.y - ry)*mTrans[6];
  float a[8];
  if (!FindAffineTransform(pLCorners, true, a)) {
    msg_info() << "Couldn't perform transfer\n";
    return false;
  }
  float posx, posy; 
  posx = a[0]*mLeftPos.x + a[1]*mLeftPos.y + a[2]*cz + a[3];
  posy = a[4]*mLeftPos.x + a[5]*mLeftPos.y + a[6]*cz + a[7];
  mLeftPos.x = posx;
  mLeftPos.y = posy;
#if !DISPTRACK_LEFTONLY
  if (!FindAffineTransform(pRCorners, true, a))
    return false;
  msg_info() << "RightPos: " << mRightPos.x << " " << rightpos[1];
  posx = a[0]*mRightPos.x + a[1]*rightpos[1] + a[2]*cz + a[3];
  posy = a[4]*mRightPos.x + a[5]*rightpos[1] + a[6]*cz + a[7];
  mRightPos.x = posx;
  rightpos[1] = posy;
#endif
#endif
  return true;
}

bool CDispTracker::FindAffineTransform(Corners &pLCorners, bool applyleft, 
  float *atrans)
{
//used only for tracking
#if 0
  Corners::CVector ptsl = pLCorners.GetCornList();
  Corners::CVector mpts;
  for (int i=0;i<ptsl.size();i++) {
    Corner *lLCorn = ptsl[i];
    Corner *lRCorn = lLCorn->stereo.match;
    Corner *pcorn = lLCorn->motion.match;
    if (lRCorn!=nullptr && lRCorn->stereo.match==lLCorn && pcorn!=nullptr && 
	pcorn->motion.match==lLCorn) {
      //msg_info() << i << ": " << lLCorn->xpos-pcorn->xpos << " " << 
      //  lLCorn->ypos-pcorn->ypos << " " << lLCorn->stereo.cZ << "\n";
      mpts.push_back(lLCorn);
      mpts.push_back(pcorn);
    }
  }
  if (mpts.size()<12) {
    msg_info() << "Too few points\n";
    return false;
  }

  for (int i=0;i<8;i++)
    atrans[i] = 0.0;
  atrans[0] = atrans[5] = 1.0;

  float sumwei = 0.0;       // Initialize translation
  float sumerr = 0.0;
  for (int i=0;i<mpts.size();i+=2) {
    mpts[i]->ypos += 0.1 * mpts[i]->stereo.cZ;
    atrans[3] += mpts[i+1]->xpos - mpts[i]->xpos;
    atrans[7] += mpts[i+1]->ypos - mpts[i]->ypos;
    sumwei++;
  }
  atrans[3] /= sumwei;
  atrans[7] /= sumwei;

  for (int loop=0;loop<3;loop++) {

    sumwei = 0.0;
    float midx = 0.0, midy = 0.0, midz = 0.0;
    float midrx = 0.0, midry = 0.0;
    for (int i=0;i<mpts.size();i+=2) {
      float predx = atrans[0]*mpts[i]->xpos + atrans[1]*mpts[i]->ypos +
	atrans[2]*mpts[i]->stereo.cZ + atrans[3];
      float predy = atrans[4]*mpts[i]->xpos + atrans[5]*mpts[i]->ypos +
	atrans[6]*mpts[i]->stereo.cZ + atrans[7];
      float dx = mpts[i+1]->xpos - predx; 
      float dy = mpts[i+1]->ypos - predy;
      float wei = 4.0 / (4.0 + dx*dx + dy*dy);
      midx += wei * mpts[i]->xpos;
      midy += wei * mpts[i]->ypos;
      midz += wei * mpts[i]->stereo.cZ;
      midrx += wei * mpts[i+1]->xpos;
      midry += wei * mpts[i+1]->ypos;
      sumwei += wei;
    }
    midx /= sumwei;
    midy /= sumwei;
    midz /= sumwei;
    midrx /= sumwei;
    midry /= sumwei;

    sumwei = 0.0;
    sumerr = 0.0;
    float aa[6], ax[3], ay[3];
    for (int i=0;i<6;i++)
      aa[i] = 0.0;
    for (int i=0;i<3;i++)
      ax[i] = ay[i] = 0.0;

    for (int i=0;i<mpts.size();i+=2) {
      float predx = atrans[0]*mpts[i]->xpos + atrans[1]*mpts[i]->ypos +
	atrans[2]*mpts[i]->stereo.cZ + atrans[3];
      float predy = atrans[4]*mpts[i]->xpos + atrans[5]*mpts[i]->ypos +
	atrans[6]*mpts[i]->stereo.cZ + atrans[7];
      float dx = mpts[i+1]->xpos - predx; 
      float dy = mpts[i+1]->ypos - predy;
      float wei = 4.0 / (4.0 + dx*dx + dy*dy);
      sumerr += 4.0*log(1.0/wei);
      float lx = mpts[i]->xpos - midx;
      float ly = mpts[i]->ypos - midy;
      float lz = mpts[i]->stereo.cZ - midz;
      aa[0] += wei * lx*lx;
      aa[1] += wei * lx*ly;
      aa[2] += wei * lx*lz;    // aa[0] aa[1] aa[2]
      aa[3] += wei * ly*ly;    // aa[1] aa[3] aa[4]
      aa[4] += wei * ly*lz;    // aa[2] aa[4] aa[5]
      aa[5] += wei * lz*lz;
      float rx = mpts[i+1]->xpos - midrx;
      ax[0] += wei * rx*lx;
      ax[1] += wei * rx*ly;
      ax[2] += wei * rx*lz;
      float ry = mpts[i+1]->ypos - midry;
      ay[0] += wei * ry*lx;
      ay[1] += wei * ry*ly;
      ay[2] += wei * ry*lz;
      sumwei += wei;
    }
    
    float iaa[6];
    iaa[0] = aa[3]*aa[5] - aa[4]*aa[4];
    iaa[1] = aa[2]*aa[4] - aa[1]*aa[5];
    iaa[2] = aa[1]*aa[4] - aa[2]*aa[3];
    iaa[3] = aa[0]*aa[5] - aa[2]*aa[2];
    iaa[4] = aa[1]*aa[2] - aa[0]*aa[4];
    iaa[5] = aa[0]*aa[3] - aa[1]*aa[1];
    float det = aa[0]*iaa[0] + aa[1]*iaa[1] + aa[2]*iaa[2];
    atrans[0] = (ax[0]*iaa[0] + ax[1]*iaa[1] + ax[2]*iaa[2]) / det;
    atrans[1] = (ax[0]*iaa[1] + ax[1]*iaa[3] + ax[2]*iaa[4]) / det;
    atrans[2] = (ax[0]*iaa[2] + ax[1]*iaa[4] + ax[2]*iaa[5]) / det;
    atrans[3] = midrx - (atrans[0]*midx + atrans[1]*midy + atrans[2]*midz);
    atrans[4] = (ay[0]*iaa[0] + ay[1]*iaa[1] + ay[2]*iaa[2]) / det;
    atrans[5] = (ay[0]*iaa[1] + ay[1]*iaa[3] + ay[2]*iaa[4]) / det;
    atrans[6] = (ay[0]*iaa[2] + ay[1]*iaa[4] + ay[2]*iaa[5]) / det;
    atrans[7] = midry - (atrans[4]*midx + atrans[5]*midy + atrans[6]*midz);
    
  }
#if 0
  msg_info() << sumerr << " \t" << sumwei << " \t";
  for (int i=0;i<8;i++)
    msg_info() << atrans[i] << " ";
  msg_info() << "\n";
  msg_info() << "Summed weights: " << sumwei << "\n";
#endif
  
  if (sumwei<10.0) {
    msg_info() << "Not enough weights\n";
    return false;
  }
#endif
  return true;
}

int CDispTracker::FindAffineFMatrix(Corners::CVector &ptsl)
{
//not used
#if 0
  const float lLimit = 9.0;
  const int minnum = 10;

  Corners::CVector mpts;
  for (int i=0;i<ptsl.size();i++) {
    if (ptsl[i]->stereo.match!=nullptr && 
	ptsl[i]->stereo.match->stereo.match==ptsl[i]) 
      mpts.push_back(ptsl[i]);
  }
  if (mpts.size()<minnum) {
    //msg_info() <<"FindAffineFMatrix(): Too few successful matches\n";
    mParams[0] = mParams[2] = mParams[4] = 0.0;
    mParams[1] = 1.0;
    mParams[2] = -1.0;
    return mpts.size();
  }
  
  int maxfit = 0;
  double ba[5];
  for (int loop=0;loop<200;loop++) {  // Use RANSAC to find initial estimate
    int idx[minnum];
    int n = 0;
    while (n<minnum) {
      idx[n] = (int)((double)mpts.size()*rand()/(RAND_MAX+1.0));
      bool exist = false;
      for (int i=0;i<n;i++)
	if (idx[n]==idx[i])
	  exist = true;
      if (!exist)
	n++;
    }    
    
    double med[4];
    for (int i=0;i<4;i++) 
      med[i] = 0.0;
    for (int i=0;i<minnum;i++) {
      Corner *lPt1 = mpts[idx[i]];
      Corner *lPt2 = mpts[idx[i]]->stereo.match;     // 0.0  0.0  a[2]
      med[0] += lPt1->xpos;                          // 0.0  0.0  a[3]
      med[1] += lPt1->ypos;                          // a[0] a[1] a[4]
      med[2] += -lPt2->xpos;
      med[3] += -lPt2->ypos;
    }
    for (int i=0;i<4;i++) 
      med[i] /= minnum;
    
    double zz[4*(4+1)/2], z[4], a[5];
    for (int i=0;i<4*(4+1)/2;i++) 
      zz[i] = 0.0;
    for (int i=0;i<4;i++) 
      a[i] = 0.0;
    for (int i=0;i<minnum;i++) {
      Corner *lPt1 = mpts[idx[i]];
      Corner *lPt2 = mpts[idx[i]]->stereo.match;
      z[0] = lPt1->xpos - med[0];
      z[1] = lPt1->ypos - med[1];
      z[2] = -lPt2->xpos - med[2];
      z[3] = -lPt2->ypos - med[3];
      int n = 0;
      for (int j=0;j<4;j++) 
	for (int k=j;k<4;k++,n++) 
	  zz[n] += z[j] * z[k];
    }
    double val = FindSymLeaEig<4>(zz, a);
    a[4] = -(a[0]*med[0] + a[1]*med[1] + a[2]*med[2] + a[3]*med[3]);
    if (fabs(a[0])>fabs(a[1]))
      continue;                  // may happen if disparity range is too low
    int numfit = 0;
    for (int i=0;i<mpts.size();i++) {
      Corner *lPt1 = mpts[i];
      Corner *lPt2 = mpts[i]->stereo.match;
      float err = lPt1->xpos*a[0] + lPt1->ypos*a[1] - lPt2->xpos*a[2] - lPt2->ypos*a[3] + a[4];
      err /= sqrt(a[0]*a[0] + a[1]*a[1]);
      if (err*err<lLimit) 
	numfit++;
    }
    //msg_info() << "A: " << numfit << " " << sqrt(2.0)*a[0] << " " << sqrt(2.0)*a[1] << " " 
    //	      << sqrt(2.0)*a[2] << " " << sqrt(2.0)*a[3] << " " << sqrt(2.0)*a[4] << " "
    //	      << sqrt(a[2]*a[2] + a[3]*a[3])/sqrt(a[0]*a[0] + a[1]*a[1]) << " " 
    //	      << atan2(a[0], a[1])-atan2(a[2], a[3]) << "\n";
    if (numfit>maxfit) {
      for (int i=0;i<5;i++) 
	ba[i] = sqrt(2.0)*a[i];
      maxfit = numfit;
    }
  }
  //msg_info() << "BA: " << maxfit << " " << ba[0] << " " << ba[1] << " " 
  //	    << ba[2] << " " << ba[3] << " " << ba[4] << " "
  //	    << sqrt(ba[2]*ba[2] + ba[3]*ba[3])/sqrt(ba[0]*ba[0] + ba[1]*ba[1]) << " "
  //	    << atan2(ba[0], ba[1])-atan2(ba[2], ba[3]) << "\n";

  int numfit;       // Improve affine F-matrix using M-estimators
  float fiterr;
  for (int loop=0;loop<4;loop++) {
    double med[5];
    for (int i=0;i<5;i++) 
      med[i] = 0.0;
    for (int i=0;i<mpts.size();i++) {
      Corner *lPt1 = mpts[i];
      Corner *lPt2 = mpts[i]->stereo.match;     
      float err = lPt1->xpos*ba[0] + lPt1->ypos*ba[1] - lPt2->xpos*ba[2] - lPt2->ypos*ba[3] + ba[4];
      float wei = (err*err<lLimit ? 1.0 : 0.0); //lLimit / (4.0*err*err + lLimit);
      med[0] += lPt1->xpos * wei;                      
      med[1] += lPt1->ypos * wei;                      
      med[2] += -lPt2->xpos * wei;
      med[3] += -lPt2->ypos * wei;
      med[4] += wei;
    }
    
    numfit = 0; 
    fiterr = 0.0;
    double zz[4*(4+1)/2], z[4];
    for (int i=0;i<4*(4+1)/2;i++) 
      zz[i] = 0.0;
    for (int i=0;i<mpts.size();i++) {
      Corner *lPt1 = mpts[i];
      Corner *lPt2 = mpts[i]->stereo.match;
      float err = lPt1->xpos*ba[0] + lPt1->ypos*ba[1] - lPt2->xpos*ba[2] - lPt2->ypos*ba[3] + ba[4];
      if (err*err<lLimit) {
	fiterr += err*err;
	numfit++;
      }
      float wei = (err*err<lLimit ? 1.0 : 0.0); //lLimit / (4.0*err*err + lLimit);
      z[0] = lPt1->xpos;
      z[1] = lPt1->ypos;
      z[2] = -lPt2->xpos;
      z[3] = -lPt2->ypos;
      int n = 0;
      for (int j=0;j<4;j++) 
	for (int k=j;k<4;k++,n++) 
	  zz[n] += (wei * z[j] * z[k]);
    }
    int n = 0;
    for (int i=0;i<4;i++) {
      for (int j=i;j<4;j++,n++) 
	zz[n] -= med[i]*med[j] / med[4];
    }
    
    double val = FindSymLeaEig<4>(zz, ba);
    ba[4] = -(ba[0]*med[0] + ba[1]*med[1] + ba[2]*med[2] + ba[3]*med[3]) / med[4];
  }
  //msg_info() << "BA: " << numfit << " " << sqrt(2.0)*ba[0] << " " << sqrt(2.0)*ba[1] << " " 
  //	    << sqrt(2.0)*ba[2] << " " << sqrt(2.0)*ba[3] << " " << sqrt(2.0)*ba[4] << " "
  //	    << sqrt(ba[2]*ba[2] + ba[3]*ba[3])/sqrt(ba[0]*ba[0] + ba[1]*ba[1]) << " "
  //	    << atan2(ba[0], ba[1])-atan2(ba[2], ba[3]) << "\n";
  mParams[0] = sqrt(2.0)*ba[0];
  mParams[1] = sqrt(2.0)*ba[1];
  mParams[2] = -sqrt(2.0)*ba[2];
  mParams[3] = -sqrt(2.0)*ba[3];
  mParams[4] = sqrt(2.0)*ba[4];
  return numfit;
#endif
  return 0;
}

bool CDispTracker::FindCenter(Corners &pLCorners, int w, int h, float pYDif, float &pXPos, float &pYPos, float &pXDif, float pPruneWei)
{
    int lLNumCorns = pLCorners.GetNumCorn();
    Corners::CVector lLCornList = pLCorners.GetCornList();

    float exr = mExpRange/mResolution;
    int dw = 8*(int)(exr)+1;
    float *lDWeiHist = new float[dw];
    for (int i=0;i<dw;i++)
        lDWeiHist[i] = 0.0;

    float iex2 = -1.0 / (2.0*mExpSize.x*mExpSize.x/mResolution/mResolution/4.0);
    float iey2 = -1.0 / (2.0*mExpSize.y*mExpSize.y/mResolution/mResolution/4.0);
    float ied2 = -1.0 / (2.0*exr*exr/4.0);
    float lWeightSum = 0.0f;
  
    for (int i=0;i<lLNumCorns;i++) 
    {
        Corner *lLCorn = lLCornList[i]; 
        if (lLCorn->stereo.match!=nullptr && lLCorn->stereo.match->stereo.match==lLCorn) 
        {
            Corner *lRCorn = lLCorn->stereo.match;
            float x = lRCorn->xpos - w/2;
            float y = lRCorn->ypos - h/2;
            float xm = (lLCorn->xpos + lRCorn->xpos - w) / 2.0;
            float ym = (lLCorn->ypos + lRCorn->ypos - h) / 2.0;
            float dx = lLCorn->xpos - (x*mTrans[0] + y*mTrans[1] + w/2);
            float lWei = exp(iex2*xm*xm + iey2*ym*ym + ied2*dx*dx/16.0);
            lWeightSum += lWei;
            int d = (int)(dx + dw/2);
      
            if (d<0) 
                lDWeiHist[0] += lWei;
            else if (d>=dw) 
                lDWeiHist[dw-1] += lWei;
            else 
                lDWeiHist[d] += lWei;
        }
    }

    float lLimit = lWeightSum / 2.0;
    lWeightSum = 0.0f;
    int i = 0;
  
    while (lWeightSum<lLimit && i<dw) 
    {
        lWeightSum += lDWeiHist[i];
        i++;
    }
  
    pXDif = 0.0f;
    if (lDWeiHist[i-1]>0.0f)
        pXDif = i - dw/2 - (lWeightSum-lLimit)/lDWeiHist[i-1];
    delete [] lDWeiHist;

    pXPos = w/2;
    pYPos = h/2;
    for (int k=0;k<3;k++) 
    {
        lWeightSum = 0.0;
        float lWeightSumx = 0.0, lWeightSumy = 0.0, lWeightSumd = 0.0; 
        for (int i=0;i<lLNumCorns;i++) 
        {
            Corner *lLCorn = lLCornList[i]; 
            if (lLCorn->stereo.match!=nullptr && lLCorn->stereo.match->stereo.match==lLCorn) 
            {
                Corner *lRCorn = lLCorn->stereo.match;
                float x = lRCorn->xpos - w/2;
                float y = lRCorn->ypos - h/2;
                float dx = lLCorn->xpos - (x*mTrans[0] + y*mTrans[1] + w/2);
                //msg_info()msg_info() << " " << dx << " " << pXDif << "\n"; 
                float xm = (lLCorn->xpos + lRCorn->xpos) / 2.0 - pXPos;
                float ym = (lLCorn->ypos + lRCorn->ypos) / 2.0 - pYPos;
                float lWei = exp(iex2*xm*xm + iey2*ym*ym + ied2*(dx-pXDif)*(dx-pXDif));
                lWeightSum += lWei;
                lWeightSumx += lWei * lLCorn->xpos;
                lWeightSumy += lWei * lLCorn->ypos;
                lWeightSumd += lWei * dx;
            }
        } 
        if (lWeightSum>0.0f) 
        {
            pXPos = lWeightSumx / lWeightSum;
            pYPos = lWeightSumy / lWeightSum;
            pXDif = lWeightSumd / lWeightSum;
        }
    }

    if (pPruneWei>0.001) 
    {
        for (int i=0;i<lLNumCorns;i++) 
        {
            Corner *lLCorn = lLCornList[i]; 
            if (lLCorn->stereo.match!=nullptr && lLCorn->stereo.match->stereo.match==lLCorn) 
            {
                Corner *lRCorn = lLCorn->stereo.match;
                float x = lRCorn->xpos - w/2;
                float y = lRCorn->ypos - h/2;
                float dx = lLCorn->xpos - (x*mTrans[0] + y*mTrans[1] + w/2);
                float xm = (lLCorn->xpos + lRCorn->xpos) / 2.0 - pXPos;
                float ym = (lLCorn->ypos + lRCorn->ypos) / 2.0 - pYPos;
                float lWei = exp(iex2*xm*xm + iey2*ym*ym + ied2*(dx-pXDif)*(dx-pXDif));
                if (lWei<pPruneWei) 
                {
                    lRCorn->stereo.match = nullptr;
                    lLCorn->stereo.match = nullptr;
                }
            }
        } 
    }

    if (i==1 || i==dw || lWeightSum<=3.0) 
    {
        pXPos = w/2;
        pYPos = h/2;
        pXDif = 0;
        return false;
    }
    else 
        return true;
}

void CDispTracker::DispSegment(Image<float> &dimg, Image<unsigned char> &cut, 
        float dsiz, float drange, float &pXPos, float &pYPos)
{
    //not used right now
    Warning() << "entering non-implemented function DispSegment\n";
#if 0
    int w = dimg.GetWidth();
    int h = dimg.GetHeight();
    assert(w==cut.GetWidth() && h==cut.GetHeight());
    float mind = (dsiz - (int)drange)/2;
    float maxd = (dsiz + (int)drange)/2;
    float *dimd = dimg.GetData();

    float meanx = 0.0;
    float meany = 0.0;
    float meand = 0.0;
    int total = 0;
    for (int y=0;y<h;y++) {
        for (int x=0;x<w;x++) {
            int p = y*w + x;
            if (dimd[p]>=mind && dimd[p]<=maxd) {
                meanx += x;
                meany += y;
                meand += dimd[p];
                total++;
            }
        }
    }
    if (total) {
        meanx /= total;
        meany /= total;
        meand /= total;
    }

    int minx, miny, maxx, maxy;
    for (int i=0;i<5;i++) {
        minx = (int)(meanx - mExpSize.x/mSubSample);
        if (minx<0) minx = 0;
        maxx = (int)(meanx + mExpSize.x/mSubSample);
        if (maxx>w) maxx = w;
        miny = (int)(meany - mExpSize.y/mSubSample);
        if (miny<0) miny = 0;
        maxy = (int)(meany + mExpSize.y/mSubSample);
        if (maxy>h) maxy = h;
        mind = meand - drange/2;
        if (mind<0.0) mind = 0.0;
        maxd = meand + drange/2;
        meanx = 0.0; 
        meany = 0.0; 
        meand = 0.0;
        total = 0;
        for (int y=miny;y<maxy;y++) {
            for (int x=minx;x<maxx;x++) {
                int p = y*w + x;
                if (dimd[p]>=mind && dimd[p]<=maxd) {
                    meanx += x;
                    meany += y;
                    meand += dimd[p];
                    total++;
                }
            }
        }
        if (total) {
            meanx /= total;
            meany /= total;
            meand /= total;
        }
    }

    minx = (int)(meanx - mExpSize.x/mSubSample);
    maxx = (int)(meanx + mExpSize.x/mSubSample);
    miny = (int)(meany - mExpSize.y/mSubSample);
    maxy = (int)(meany + mExpSize.y/mSubSample);
    unsigned char *cutd = cut.GetData();
    int numset = 0;
    for (int y=0;y<h;y++) {
        for (int x=0;x<w;x++) {
            int p = y*w + x;
            if (dimd[p]>=mind && dimd[p]<=maxd && x>=minx && x<maxx && 
                    y>=miny && y<maxy) {
                cutd[p] = 255; //(unsigned char)(255*(dimd[p]-mind) / dsiz);
                numset++;
            } else if (dimd[p]<0.0) 
                cutd[p] = 0;
            else 
                cutd[p] = 0;
        } 
    }
    float density = (float)numset / (mExpSize.x * mExpSize.y) * 
        (mSubSample * mSubSample) / 4;
    if (density>0.1) {
        pXPos = meanx * mSubSample / 2;
        pYPos = meany * mSubSample / 2;
    }
    //msg_info() << "mean: " << meanx << " " << meany << " " << meand << " " << density << "\n";
#endif
}

#endif

/*namespace cvi*/ } /*namespace buola*/ }

